/**
 * Snowflake MEGA sumo robot strategy nr.4 - first move is to retrieve backwards to the line
 * Copyright 2013 Maris BECS <maris.becs@gmail.com>
 */

#include <asf.h>
#include "strategy4.h"
#include "keyboard.h"
#include "sensors.h"
#include "motors.h"


void strategy4_app(void)
{
	struct keyboard_event input;

	// set speed for the strategy
	int8_t speed = 100;
	int8_t frc = -10;	// forward speed correction for the right motor
	bool first_move_dir_left = true;

	// show select info
	gfx_mono_draw_string("Drive backward & then:", 0, 14, &sysfont);
	gfx_mono_draw_string("ENTER-Left,  UP-Right", 0, 24, &sysfont);

	// Set the PWM
	while (true) {
		keyboard_get_key_state(&input);
		if (input.type == KEYBOARD_RELEASE) {
			if (input.keycode == KEYBOARD_ENTER) {
				first_move_dir_left = true;
				break;
			} else if (input.keycode == KEYBOARD_UP) {
				first_move_dir_left = false;
				break;
			} else if (input.keycode == KEYBOARD_BACK) {
				// set IR module back to OFF state
				ioport_set_pin_high(START_SW);
				// go back to FIGHT menu
				return;
			}
		}
	}

	// clear screen
	gfx_mono_draw_filled_rect(0, 0, 128, 32, GFX_PIXEL_CLR);

	if (first_move_dir_left) {
		// show selected move info
		gfx_mono_draw_string("Strategy 4   drive-BL", 0, 0, &sysfont);
		gfx_mono_draw_string(" Waiting for IR start", 0, 24, &sysfont);
	} else {
		// show selected move info
		gfx_mono_draw_string("Strategy 4   drive-BR", 0, 0, &sysfont);
		gfx_mono_draw_string(" Waiting for IR start", 0, 24, &sysfont);
	}

	while (true) {
		// wait for IR remote start signal
		if ( HIGH == ioport_get_pin_level(REMOTE_START) ){
			break;
		}
	}

	// drop the flag
	turn_servo(true);

	// turn on power for sensors
	sensorPower(true);

	// robot better turns left direction so right turn time needs to be bigger
	if (first_move_dir_left) {
		// turn right
		drive( speed, -speed );
		delay_ms(60); // <========== Calibrate this value!!! ===========
	} else {
		// turn left
		drive( -speed, speed );
		delay_ms(50); // <========== Calibrate this value!!! ===========
	}

	while (true) {
		Line_sens = line_read();
		drive( -speed, -speed ); // <========== Calibrate this value if necessary !!! ===========
		// check kill switch state
		if ( (LOW == ioport_get_pin_level(KILL_SW)) || (LOW == ioport_get_pin_level(REMOTE_KILL)) ) {
			drive(0,0);
			break;
		}
		if ( (Line_sens&0b00000001) || (Line_sens&0b00001000) ){		// Line_BR or Line_BL
			delay_us(500);
			Line_sens = line_read();
			if ( (Line_sens&0b00000001) || (Line_sens&0b00001000) ){	// Line_BR or Line_BL
				break;
			}
		}
	}

	drive(0,0);	// stop/break
	delay_ms(50);
	drive(100,100);	// drive little forward
	delay_ms(35);	// <========== Calibrate this value if necessary !!! ===========


	while (true)
	{
		// check kill switch state
		if ( (LOW == ioport_get_pin_level(KILL_SW)) || (LOW == ioport_get_pin_level(REMOTE_KILL)) ) {
			// kill switch has been activated
			drive(0,0);
			// set IR module back to OFF state
			ioport_set_pin_high(START_SW);
			// turn off power for sensors
			sensorPower(false);
			pwm_start( &pwm_cfg[1], 0 ); // free the servo
			// clear screen
			gfx_mono_draw_filled_rect(0, 0, 128, 32, GFX_PIXEL_CLR);
			return;
		}

		// while loop sensor read delay
		delay_ms(3);

		// read all sensors
		Line_sens = line_read();
		Distance_sens = sensor_read();

		// drive straight with right motor correction !!!
		drive( speed, speed+frc );


		/*
		 * ######################### Check line sensor state #########################
		 */
		if( (Line_sens&0b00000010) || (Line_sens&0b00000100) ){	// Line_FL or Line_FR
			delay_us(500);
			Line_sens = line_read();
			// Line_FL
			if (Line_sens&0b00000010) {
				drive(0,0);	// stop
				delay_ms(50);
				// turn right while NOT see line again
				while ( !(Line_sens&0b00000001) ){	// Line_BR
					Line_sens = line_read();
					drive(speed,-speed);
					// check kill switch state
					if ( (LOW == ioport_get_pin_level(KILL_SW)) || (LOW == ioport_get_pin_level(REMOTE_KILL)) ) {
						drive(0,0);
						break;
					}
				}
			// Line_FR
			} else if (Line_sens&0b00000100) {
				drive(0,0);	// stop
				delay_ms(50);
				// turn left while NOT see line again
				while ( !(Line_sens&0b00001000) ){	// Line_BL
					Line_sens = line_read();
					drive(-speed,speed);
					// check kill switch state
					if ( (LOW == ioport_get_pin_level(KILL_SW)) || (LOW == ioport_get_pin_level(REMOTE_KILL)) ) {
						drive(0,0);
						break;
					}
				}
			}
		}


		/*
		 * ######################### Check distance sensor state #########################
		 */
		if (Distance_sens&0b00010000) {		// L10
			drive( speed*0.7, speed );	// drive little left
			delay_ms(5);

		} else if (Distance_sens&0b00001000) {	// R10
			drive( speed, speed*0.7 );	// drive little right
			delay_ms(5);

		} else if (Distance_sens&0b00100000) {	// L30
			drive( speed/3, speed );	// drive more left
			delay_ms(5);

		} else if (Distance_sens&0b00000100) {	// R30
			drive( speed, speed/3 );	// drive more right
			delay_ms(5);

		} else if (Distance_sens&0b01000000) {	// L45
			drive( -speed, speed );	// turn on spot left
			delay_ms(5);

		} else if (Distance_sens&0b00000010) {	// R45
			drive( speed, -speed );	// turn on spot right
			delay_ms(5);

		} else if (Distance_sens&0b10000000) {	// L90
			drive( -speed, speed );	// turn on spot left
			delay_ms(5);

		} else if (Distance_sens&0b00000001) {	// R90
			drive( speed, -speed );	// turn on spot right
			delay_ms(5);
		} /*else if ( HIGH == ioport_get_pin_level(PEPPERL_FUCHS) ){
			drive( speed, speed+frc );	// drive straight full speed, not sure if necessary !!!
			delay_ms(5);
		}*/


	}
}